# 导入必要的库和模块
import rospy
import time
from action_t import Action_t
import math

# 初始化 Action_t 类，使用仿真模式 "sim"
drone = Action_t("sim")

# 用于记录上一个已打印入口消息的状态
last_printed_state = -1

print("开始执行 e4.py: 解锁 -> 起飞 -> 向前1m -> 向右1m -> 降落 -> 上锁 (使用内部状态机和单一标志位)")

# 主循环
while not rospy.is_shutdown():
    # 状态机处理
    current_state = drone.state_machine_current_state

    if current_state == 0:  # 解锁状态
        if last_printed_state != current_state:
            print("状态 0: 解锁无人机")
            last_printed_state = current_state
        
        # 执行解锁操作 (无需检查线程状态)
        drone.unlock(use_thread=True)
        
        if drone.control_complete():
            print("无人机已解锁")
            drone.state_machine_current_state = 1

    elif current_state == 1:  # 起飞状态
        if last_printed_state != current_state:
            print("状态 1: 起飞到 1.2 米")
            last_printed_state = current_state
        
        # 执行起飞操作 (无需检查线程状态)
        drone.send_position_x_y_z_t(0, 0, 1.2, 4, use_thread=True)
        
        if drone.control_complete():
            print("无人机已起飞")
            drone.state_machine_current_state = 2

    elif current_state == 2: # 向前飞行状态
        if last_printed_state != current_state:
            print("状态 2: 向前飞行 1 米")
            last_printed_state = current_state
        
        # 执行向前飞行操作 (无需检查线程状态)
        drone.send_position_x_y_z_t(1, 0, 1.2, 4, use_thread=True) # 到达 (1, 0, 1.2)
        
        if drone.control_complete():
            print("无人机已向前飞行 1 米")
            drone.state_machine_current_state = 3

    elif current_state == 3: # 向右飞行状态
        if last_printed_state != current_state:
            print("状态 3: 向右飞行 1 米")
            last_printed_state = current_state
        
        # 执行向右飞行操作 (无需检查线程状态)
        drone.send_position_x_y_z_t(1, -1, 1.2, 4, use_thread=True) # 4秒内到达(1, -1, 1.2)
        
        if drone.control_complete():
            print("无人机已向右飞行 1 米")
            drone.state_machine_current_state = 4

    elif current_state == 4:  # 降落状态
        if last_printed_state != current_state:
            print("状态 4: 自动降落")
            last_printed_state = current_state
        
        # 执行降落操作 (无需检查线程状态)
        drone.land_auto(use_thread=True)
        
        if drone.control_complete():
            print("无人机已降落")
            drone.state_machine_current_state = 5

    elif current_state == 5:  # 上锁状态
        if last_printed_state != current_state:
            print("状态 5: 上锁无人机")
            last_printed_state = current_state
        
        # 执行上锁操作 (无需检查线程状态)
        drone.lock(use_thread=True)
        
        if drone.control_complete():
            print("无人机已上锁")
            drone.state_machine_current_state = 6

    elif current_state == 6:  # 完成状态
        if last_printed_state != current_state:
            print("状态 6: 所有动作完成")
            last_printed_state = current_state

    # 短暂休眠
    time.sleep(0.1)

print("e4.py 执行完毕") 